package Behaviors.Schemas.Perception;
import Robot.*;
import Robot.Sensors.Sensor;
import Robot.Sensors.Sonar;
import Utils.Vector2d;
import Utils.Pose2d;


//Corregir que este ubicando los obstaculos con referencia global. Dejarlo como referencia
//local
public class PSDetectObstaclesSonar extends PSDetectObstacles {
    
    Sonar sonar;
    Position2d location;
    double [] poses;
    Vector2d[] data;

    boolean irMode = false;
    
    public PSDetectObstaclesSonar(String name,Position2d location, Sonar sonar,boolean irMode) {
        super(name);
        this.sonar=sonar;
        this.location = location;
        this.irMode = irMode;

        poses=sonar.getPoses();
        data = new Vector2d[poses.length];

        this.maxRange = 0.6f;
        this.minRange = 0.2f;

        


        
    }

    public Vector2d[] Value() {
        
        
        float [] dataTemp=sonar.read(Sensor.READ_MODE_NO_WAIT);
        Pose2d locPose = location.read(Sensor.READ_MODE_NO_WAIT);
        //as the IRs we have cannot sense distance ranges, set vector mag. as MIN_RANGE.
        for(int i=0;i<data.length;i++){
           
            data[i] = new Vector2d();

            //The location of the obstacle is with respect to origin (0,0) define theta with respect to global axes
            data[i].setAngle((float)poses[i]+ (float)locPose.getPa());

            //if there is an obstacle below the MIN_RANGE, signal it.
            if(dataTemp[i]<=minRange) {

                //add robot radius to distance
                data[i].setMag(minRange+Robot.radius);

                //Translate vector from robot coordinates to global coordinates
                data[i] = data[i].add(location.getPosition());
            }
            //Aqui irian las estrategias para que el Sonar no se comporte de manera
            //discreta
            else if(irMode)//if(dataTemp[i] < maxRange)
                data[i].setMag(0);
            else
            {
                if(dataTemp[i]<=maxRange)
                {
                    data[i].setMag(dataTemp[i]+Robot.radius);
                    data[i] = data[i].add(location.getPosition());
                }
                else
                    data[i].setMag(0);
            }

        }
        
        return data;
    }
}
